// *********************************************************
//      Copyright (c) Kosta Gaitanis
//      All rights reserved
//
//      k.gaitanis@gmail.com
//
// *********************************************************

#include <cmath>
#include "Matrix.h"

// Assign values to predefined matrices.
const Matrix Matrix::Identity(1.0);

Matrix::Matrix(
                 double s00, double s01, double s02, double s03,
                 double s10, double s11, double s12, double s13,
                 double s20, double s21, double s22, double s23,
                 double s30, double s31, double s32, double s33
                 )
{
    m[0][0] = s00; m[0][1] = s01; m[0][2] = s02; m[0][3] = s03;
    m[1][0] = s10; m[1][1] = s11; m[1][2] = s12; m[1][3] = s13;
    m[2][0] = s20; m[2][1] = s21; m[2][2] = s22; m[2][3] = s23;
    m[3][0] = s30; m[3][1] = s31; m[3][2] = s32; m[3][3] = s33;
}

Matrix::Matrix(double d)
{
    memset(m, 0, sizeof(double) * 16);
    // all elements are now set to 0

    //START//
    // set the value of all the diagonal elements to d
    m[0][0] = d; m[1][1] = d; m[2][2] = d; m[3][3] = d;
    //END//
}

Matrix::Matrix(const Matrix &matrix)
{
    memcpy((void*)&m, &matrix.m, sizeof(double) * 16);
}

Matrix& Matrix::operator = (const Matrix &matrix)
{
    if (this != &matrix)
        memcpy((void*)&m, &matrix.m, sizeof(double) * 16);
    return *this;
}

const double* Matrix::operator [] (int i) const
{
    return m[i];
}

double Matrix::get(int row, int col) const
{
    return m[row][col];
}

void Matrix::set(int row, int col, double value)
{
    m[row][col] = value;
}

double Matrix::operator() (int row, int col) const
{
    return m[row][col];
}

Matrix Matrix::operator * (const Matrix &matrix) const
{
    Matrix result;
    //START//
    // perform matrix multiplication and return the new resulting matrix
    for (int i = 0; i < 4; i++)
        for (int j = 0; j < 4; j++) {
            double v = 0.0;
            for (int k = 0; k < 4; k++) {
                v += m[i][k] * matrix.m[k][j];
            }
            result.m[i][j] = v;
        }
    //END//
    return result;
}

Vector Matrix::operator * (const Vector &vector) const
{
    //START//
    // perform matrix vector multiplication (this is a vector transformation)
    // and return resulting vector
    return Vector(
                   m[0][0]*vector.x() + m[0][1]*vector.y() + m[0][2]*vector.z() + m[0][3],
                   m[1][0]*vector.x() + m[1][1]*vector.y() + m[1][2]*vector.z() + m[1][3],
                   m[2][0]*vector.x() + m[2][1]*vector.y() + m[2][2]*vector.z() + m[2][3]
                   );
    //INSERT//return Vector();
    //END//
}

void Matrix::toGLArray(GLdouble *array) const
{
    //START//
    // insert the elements of this matrix into array (which should be of length 16)
    // OpenGL is row-major whereas in Mathematics matrices are column-major.
    int idx = 0;
    for (int i = 0; i < 4; i++)
        for (int j = 0; j < 4; j++)
            array[idx++] = (GLdouble)m[j][i];

    //END//
}

Matrix Matrix::fromGLArray(const GLdouble *array)
{
    Matrix matrix;
    //START//
    // insert the elements of array (length 16) in matrix and return it
    // OpenGL is row-major whereas in Mathematics matrices are column-major.
    int idx = 0;
    for (int i = 0; i < 4; i++)
        for (int j = 0; j < 4; j++)
            matrix.m[j][i] = array[idx++];

    //END//
    return matrix;
}

bool Matrix::operator == (const Matrix &matrix) const
{
    //START//
    // verify if all the elements of this matrix are the same as the
    // the equivalent elements of the other matrix.
    // use the static Vector::compare() method
    for (int i = 0; i < 4; i++)
        for (int j = 0; j < 4; j++)
            if (!Vector::compare(m[i][j], matrix.m[i][j]))
                return false;
    return true;
    //INSERT//return false;
    //END//
}

bool Matrix::operator != (const Matrix &matrix) const
{
    return !(operator ==(matrix));
}

Matrix Matrix::transposed() const
{
    Matrix mat;
    //START//
    // enter the transposed values of this matrix into mat.
    // element (i,j) becomes (j,i) in the new matrix
    for (int i = 0; i < 4; i++)
        for (int j = 0; j < 4; j++)
            mat.m[i][j] = m[j][i];
    //END//
    return mat;
}

Matrix Matrix::translation(double x , double y , double z)
{
    //START//
    // create a new translation matrix and return it
    return Matrix(
                1.0,    0.0,    0.0,    x,
                0.0,    1.0,    0.0,    y,
                0.0,    0.0,    1.0,    z,
                0.0,    0.0,    0.0,    1.0
                );
    //INSERT//return Matrix();
    //END//
}

Matrix Matrix::translation(const Vector &vector)
{
    return translation(vector.x(), vector.y(), vector.z());
}

Matrix Matrix::rotationX(double angle)
{
    //START//
    // create a new rotation matrix around the X-axis and return it
    double sina = sin(angle);
    double cosa = cos(angle);

    return Matrix(
                1.0,    0.0,    0.0,    0.0,
                0.0,    cosa,   -sina,  0.0,
                0.0,    sina,   cosa,   0.0,
                0.0,    0.0,    0.0,    1.0
            );
    //INSERT//return Matrix();
    //END//
}

Matrix Matrix::rotationY(double angle)
{
    //START//
    // create a new rotation matrix around the Y-axis and return it
    double sina = sin(angle);
    double cosa = cos(angle);

    return Matrix(
                cosa,   0.0,    sina,   0.0,
                0.0,    1.0,    0.0,    0.0,
                -sina,  0.0,    cosa,   0.0,
                0.0,    0.0,    0.0,    1.0
            );
    //INSERT//return Matrix();
    //END//
}

Matrix Matrix::rotationZ(double angle)
{
    //START//
    // create a new rotation matrix around the Z-axis and return it
    double sina = sin(angle);
    double cosa = cos(angle);

    return Matrix(
                cosa,   -sina,  0.0,    0.0,
                sina,   cosa,   0.0,    0.0,
                0.0,    0.0,    1.0,    0.0,
                0.0,    0.0,    0.0,    1.0
            );
    //INSERT//return Matrix();
    //END//
}

Matrix Matrix::rotation(double angle, double x, double y, double z)
{
    // (optional) this is a bonus exercise, don't spend too much time here...

    //START//
    // create a new rotation matrix around the given vector and return it
    double cosa = cos(angle);
    double sina = sin(angle);

    // make sure that (x,y,z) is normalized (length = 1)
    double l = sqrt(x * x + y * y + z * z);
    double nx = x/l;
    double ny = y/l;
    double nz = z/l;

    return Matrix(
            nx * nx + (1 - nx * nx) * cosa,
            nx * ny * (1 - cosa) - nz * sina,
            nx * nz * (1 - cosa) + ny * sina,
            0.0,

            nx * ny * (1 - cosa) + nz * sina,
            ny * ny + (1 - ny * ny) * cosa,
            ny * nz * (1 - cosa) - nx * sina,
            0.0,

            nx * nz * (1 - cosa) - ny * sina,
            ny * nz * (1 - cosa) + nx * sina,
            nz * nz + (1 - nz * nz) * cosa,
            0.0,

            0.0, 0.0, 0.0, 1.0
                );

    //INSERT//return Matrix();
    //END//
}

Matrix Matrix::rotation(double angle, const Vector &vector)
{
    return Matrix::rotation(angle, vector.x(), vector.y(), vector.z());
}

QString Matrix::toString() const
{
    QString str("");

    for (int i = 0; i < 4; i++)
        str.append(QString("\t%1,\t%2,\t%3,\t%4,\n").arg(m[i][0]).arg(m[i][1]).arg(m[i][2]).arg(m[i][3]));
    // remove last comma
    str.chop(2);
    str.append("\n");
    return str;
}

QDebug operator <<(QDebug dbg, const Matrix &matrix)
{
    dbg.nospace() << "Matrix(" << matrix.toString() << ")\r\n";
    return dbg.space();
}

